/*
Copyright (c) 2010 Yunus Kara

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "stdafx.h"
#include "NavMesher.h"
#include "../GraphicsSystem.h"
#include "../PhysicsSystem.h"
#include "../GameState.h"
#include "../StateSystem.h"
#include "../Level.h"
#include "World.h"
using namespace Ogre;

NavMesher::NavMesher()
{
	//debuggers
	mDebuggerNode = GSYS->GetSceneMgr()->getRootSceneNode()->createChildSceneNode();
	AxisAlignedBox aabInf;
	aabInf.setInfinite();

	mNavMeshDebugger = GSYS->GetSceneMgr()->createManualObject("mNavMesherDebugger");
	mNavMeshDebugger->setCastShadows(false);
	mDebuggerNode->attachObject(mNavMeshDebugger);
	MaterialPtr mNavMeshDebuggerMaterial = MaterialManager::getSingleton().create("mDebuggerMaterial","General"); 
	mNavMeshDebuggerMaterial->setReceiveShadows(false); 
	mNavMeshDebuggerMaterial->getTechnique(0)->setLightingEnabled(false);
	mNavMeshDebugger->setBoundingBox(aabInf);
}

NavMesher::~NavMesher()
{
	Clear();

	mDebuggerNode->detachAllObjects();
	mDebuggerNode->getParentSceneNode()->removeAndDestroyChild(mDebuggerNode->getName());
	mDebuggerNode = NULL;

	mNavMeshDebugger->clear();
	GSYS->GetSceneMgr()->destroyManualObject(mNavMeshDebugger);
	mNavMeshDebugger = NULL;

	delete m_pmesh;
	m_pmesh = NULL;

	delete m_dmesh;
	m_dmesh = NULL;

	delete m_navMesh;
	m_navMesh = NULL;
}

void NavMesher::Clear()
{
	allPositions.clear();
}

bool NavMesher::Build(rcContext *rContext)
{
	//reset timer
	Ogre::Timer tm;
	tm.reset();
	unsigned long stime = tm.getMicroseconds();
	//clear existing
	Clear();

	//
	// Step 1. Initialize build config.
	//
	float m_cellSize = 0.3;
	float m_cellHeight = 0.2;
	float m_agentMaxSlope = 45;
	float m_agentHeight = 2.0;
	float m_agentMaxClimb = 0.9;
	float m_agentRadius = 0.6;
	float m_edgeMaxLen = 12;
	float m_edgeMaxError = 1.3;
	float m_regionMinSize = 50;
	float m_regionMergeSize = 20;
	float m_vertsPerPoly = 6;
	float m_detailSampleDist = 6;
	float m_detailSampleMaxError = 1;
	bool m_keepInterResults = false;

	// Init build configuration from GUI
	rcConfig m_cfg;	
	memset(&m_cfg, 0, sizeof(m_cfg));
	m_cfg.cs = m_cellSize;
	m_cfg.ch = m_cellHeight;
	m_cfg.walkableSlopeAngle = m_agentMaxSlope;
	m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch);
	m_cfg.walkableClimb = (int)ceilf(m_agentMaxClimb / m_cfg.ch);
	m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs);
	m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
	m_cfg.maxSimplificationError = m_edgeMaxError;
	m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize);
	m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize);
	m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
	m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
	m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError;
	
	// Set the area where the navigation will be build.
	// Here the bounds of the input mesh are used, but the
	// area could be specified by an user defined box, etc.
	rcVcopy(m_cfg.bmin, LVL->m_bmin);
	rcVcopy(m_cfg.bmax, LVL->m_bmax);
	rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);

	//
	// Step 2. Rasterize input polygon soup.
	//
	// Allocate voxel heighfield where we rasterize our input data to.
	rcHeightfield *m_solid = new rcHeightfield();

	if (!m_solid)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
		return false;
	}
	if (!rcCreateHeightfield(rContext,*m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
		return false;
	}
	
	// Allocate array that can hold triangle flags.
	// If you have multiple meshes you need to process, allocate
	// and array which can hold the max number of triangles you need to process.
	unsigned char* m_triflags = new unsigned char[LVL->m_ntris];
	if (!m_triflags)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'triangleFlags' (%d).", LVL->m_ntris);
		return false;
	}

	// Find triangles which are walkable based on their slope and rasterize them.
	// If your input data is multiple meshes, you can transform them here, calculate
	// the flags for each of the meshes and rasterize them.
	memset(m_triflags, 0, LVL->m_ntris*sizeof(unsigned char));
	rcMarkWalkableTriangles(rContext,m_cfg.walkableSlopeAngle, LVL->m_verts, LVL->m_nverts, LVL->m_tris, LVL->m_ntris, m_triflags);
	rcRasterizeTriangles(rContext,LVL->m_verts, LVL->m_nverts, LVL->m_tris, m_triflags, LVL->m_ntris, *m_solid);

	if (!m_keepInterResults)
	{
		delete [] m_triflags;
		m_triflags = 0;
	}

	//
	// Step 3. Filter walkables surfaces.
	//
	
	// Once all geoemtry is rasterized, we do initial pass of filtering to
	// remove unwanted overhangs caused by the conservative rasterization
	// as well as filter spans where the character cannot possibly stand.
	rcFilterLedgeSpans(rContext,m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
	rcFilterWalkableLowHeightSpans(rContext,m_cfg.walkableHeight, *m_solid);

	//
	// Step 4. Partition walkable surface to simple regions.
	//

	// Compact the heightfield so that it is faster to handle from now on.
	// This will result more cache coherent data as well as the neighbours
	// between walkable cells will be calculated.
	rcCompactHeightfield* m_chf = new rcCompactHeightfield();
	if (!m_chf)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
		return false;
	}
	if (!rcBuildCompactHeightfield(rContext,m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
		return false;
	}
	
	if (!m_keepInterResults)
	{
		delete m_solid;
		m_solid = 0;
	}
	
	// Prepare for region partitioning, by calculating distance field along the walkable surface.
	if (!rcBuildDistanceField(rContext,*m_chf))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
		return false;
	}

	// Partition the walkable surface into simple regions without holes.
	if (!rcBuildRegions(rContext,*m_chf, m_cfg.borderSize, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not build regions.");
	}

	//
	// Step 5. Trace and simplify region contours.
	//
	
	// Create contours.
	rcContourSet* m_cset = new rcContourSet;
	if (!m_cset)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
		return false;
	}
	if (!rcBuildContours(rContext,*m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
		return false;
	}

	//
	// Step 6. Build polygons mesh from contours.
	//
	
	// Build polygon navmesh from the contours.
	m_pmesh = new rcPolyMesh;
	if (!m_pmesh)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
		return false;
	}
	if (!rcBuildPolyMesh(rContext,*m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
		return false;
	}

	//
	// Step 7. Create detail mesh which allows to access approximate height on each polygon.
	//
	
	m_dmesh = new rcPolyMeshDetail;
	if (!m_dmesh)
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
		return false;
	}

	if (!rcBuildPolyMeshDetail(rContext,*m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
	}

	if (!m_keepInterResults)
	{
		delete m_chf;
		m_chf = 0;
		delete m_cset;
		m_cset = 0;
	}

	// At this point the navigation mesh data is ready, you can access it from m_pmesh.
	// See rcDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
	
	//
	// (Optional) Step 8. Create Detour data from Recast poly mesh.
	//
	
	// The GUI may allow more max points per polygon than Detour can handle.
	// Only build the detour navmesh if we do not exceed the limit.
	unsigned char* navData = 0;
	dtNavMeshCreateParams * m_params = new dtNavMeshCreateParams;
	int navDataSize = 0;
	if (!dtCreateNavMeshData(m_params, &navData, &navDataSize))
	{
		if (rContext)
			rContext->log(RC_LOG_ERROR, "Could not build Detour navmesh.");
		return false;
	}
	
	m_navMesh = new dtNavMesh;
	if (!m_navMesh)
	{
		delete [] navData;
		if (rContext)
			rContext->log(RC_LOG_ERROR, "Could not create Detour navmesh");
		return false;
	}
	
	if (!m_navMesh->init(navData, navDataSize, true))
	{
		delete [] navData;
		if (rContext)
			rContext->log(RC_LOG_ERROR, "Could not init Detour navmesh");
		return false;
	}

	//take time
	stime = tm.getMicroseconds() - stime;

	//fill debugger
	mNavMeshDebugger->clear();
	mNavMeshDebugger->begin("mNavMesherDebuggerMaterial", Ogre::RenderOperation::OT_LINE_LIST);

	const int nvp = m_pmesh->nvp;
	const float cs = m_pmesh->cs;
	const float ch = m_pmesh->ch;
	const float* orig = m_pmesh->bmin;

	for (int i = 0; i < m_pmesh->npolys; ++i)
	{
		const unsigned short* p = &m_pmesh->polys[i*nvp*2];
		unsigned short vi[3];
		for (int j = 2; j < nvp; ++j)
		{
			if (p[j] == 0xffff) break;
			vi[0] = p[0];
			vi[1] = p[j-1];
			vi[2] = p[j];
			for (int k = 0; k < 3+1; ++k)
			{
				const unsigned short* v = &m_pmesh->verts[vi[k%3]*3];
				const float x = orig[0] + v[0]*cs;
				const float y = orig[1] + (v[1]+1)*ch;
				const float z = orig[2] + v[2]*cs;
				Vector3 cpos(x, y, z);
				mNavMeshDebugger->position(cpos.x, cpos.y, cpos.z);
				mNavMeshDebugger->colour(1,1,1);
			}
			Vector3 tpos = Vector3::ZERO;
			for (int k = 0; k < 3; ++k)
			{
				const unsigned short* v = &m_pmesh->verts[vi[k]*3];
				const float x = orig[0] + v[0]*cs;
				const float y = orig[1] + (v[1]+1)*ch;
				const float z = orig[2] + v[2]*cs;
				Vector3 cpos(x, y, z);
				tpos += cpos;
			}
			tpos /= 3;
			allPositions.push_back(tpos);

			mNavMeshDebugger->position(tpos.x, tpos.y, tpos.z);
			mNavMeshDebugger->colour(1,1,1);
			mNavMeshDebugger->position(tpos.x, tpos.y+0.25, tpos.z);
			mNavMeshDebugger->colour(1,1,1);
		}
	}

	for (int i = 0; i < m_pmesh->npolys; ++i)
	{
		const unsigned short* poly = &m_pmesh->polys[i*m_pmesh->nvp*2];
		for (int j = 0; j < m_pmesh->nvp; ++j)
		{
			if (poly[j] == 0xffff) break;
			if (poly[m_pmesh->nvp+j] == 0xffff) continue;
			int vi[2];
			vi[0] = poly[j];
			if (j+1 >= m_pmesh->nvp || poly[j+1] == 0xffff)
				vi[1] = poly[0];
			else
				vi[1] = poly[j+1];
			for (int k = 0; k < 2; ++k)
			{
				const unsigned short* v = &m_pmesh->verts[vi[k]*3];
				const float x = orig[0] + v[0]*cs;
				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
				const float z = orig[2] + v[2]*cs;
				Vector3 cpos(x, y, z);
				mNavMeshDebugger->position(cpos.x, cpos.y, cpos.z);
				mNavMeshDebugger->colour(0,1,0);
			}
		}
	}

	for (int i = 0; i < m_pmesh->npolys; ++i)
	{
		const unsigned short* poly = &m_pmesh->polys[i*nvp*2];
		for (int j = 0; j < nvp; ++j)
		{
			if (poly[j] == 0xffff) break;
			if (poly[nvp+j] != 0xffff) continue;
			int vi[2];
			vi[0] = poly[j];
			if (j+1 >= nvp || poly[j+1] == 0xffff)
				vi[1] = poly[0];
			else
				vi[1] = poly[j+1];
			for (int k = 0; k < 2; ++k)
			{
				const unsigned short* v = &m_pmesh->verts[vi[k]*3];
				const float x = orig[0] + v[0]*cs;
				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
				const float z = orig[2] + v[2]*cs;
				Vector3 cpos(x, y, z);
				mNavMeshDebugger->position(cpos.x, cpos.y, cpos.z);
				mNavMeshDebugger->colour(0,0,1);
			}
		}
	}

	mNavMeshDebugger->end();
	return true;
}

void NavMesher::GetPath(Ogre::Vector3 from, Ogre::Vector3 to, Path* &path)
{
	//find a path
	int MAX_POLYS = 256;
	dtPolyRef m_polys[256];
	float m_straightPath[256*3];

	float m_polyPickExt[3];
	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;
	
	float m_spos[3];
	m_spos[0] = from.x;
	m_spos[1] = from.y;
	m_spos[2] = from.z;
	
	float m_epos[3];
	m_epos[0] = to.x;
	m_epos[1] = to.y;
	m_epos[2] = to.z;
	
//	dtPolyRef m_startRef = m_navMesh->getPolyArea(m_spos, m_polyPickExt);
//	dtPolyRef m_endRef   = m_navMesh->findNearestPoly(m_epos, m_polyPickExt);
	/*
	int m_npolys = m_navMesh->findPath(m_startRef, m_endRef, m_spos, m_epos, m_polys, MAX_POLYS);
	if (m_npolys > 0)
	{
		int m_nstraightPath = m_navMesh->findStraightPath(m_spos, m_epos, m_polys, m_npolys, m_straightPath, MAX_POLYS);
		path->Clear();
		for (int i = 0; i < m_nstraightPath; ++i)
		{
			Vector3 cpos(m_straightPath[i*3], m_straightPath[i*3+1]+0.25, m_straightPath[i*3+2]);
			PtfNode node(cpos);
			path->AddNode(node);
		}
	}
	*/
}

bool NavMesher::CanBeWalkedTo(Ogre::Vector3 to, Ogre::Vector3 extents)
{
	int MAX_POLYS = 256;
	dtPolyRef m_polys[256];

	float center[3];
	center[0] = to.x;
	center[1] = to.y;
	center[2] = to.z;
	
	float extent[3];
	extent[0] = extents.x;
	extent[1] = extents.y;
	extent[2] = extents.z;

	//int m_npolys = m_navMesh->(center, extent, m_polys, MAX_POLYS);
	//return m_npolys != 0;
	return 0;
}